// GAIT.C
#include "GenericTypeDefs.h"
#include "uart.h"
#include "tripod.h"
#include "gait.h"
#include "gp2d12.h"
#include "buffer.h"
#include "input.h"

unsigned int state = IDLE;
unsigned int direction;
unsigned int totalSteps;
unsigned int stepsCompleted;
unsigned int innerState;

void walkForward(){
        updateGP2D12();
	if(state == IDLE && getGP2D12Distance() > 25.0){
		state = MOVING;
		direction = FORWARD;
		innerState = 0;
		nextStep();
	}
}

void rotateCW(){
	if(state == IDLE){
		state = MOVING;
		direction = CLOCKWISE;
		innerState = 0;
		nextStep();
	}
}

void rotateCCW(){
	if(state == IDLE){
		state = MOVING;
		direction = CCLOCKWISE;
		innerState = 0;
		nextStep();
	}
}

void resetPos(){
	if(state == IDLE)
		setState(lowCenter, lowCenter);
}

void nextStep(){
	if(state == MOVING){

		if(innerState > 5){
			state = IDLE;
			return;
		}
		state = SEND_DATA;
		if(direction == FORWARD){
			switch(innerState){
				case 0: setState(highCenter, lowCenter); break;
				case 1: setState(lowFront, lowBack); break;
				case 2: setState(lowCenter, highCenter); break;
				case 3: setState(lowBack, lowFront); break;
				case 4: setState(highCenter, lowCenter); break;
				case 5: setState(lowCenter, lowCenter); break;
				default: setState(lowCenter, lowCenter);
			}
			moveIt();

                        if(innerState == 3){
                                innerState = 0;
                        }else
                                innerState++;

		} else if(direction == CLOCKWISE){
			switch(innerState){
				case 0: setState(highCenter, lowCenter); break;
				case 1: setState(lowRight, lowLeft); break;
				case 2: setState(lowCenter, highCenter); break;
				case 3: setState(lowLeft, lowRight); break;
				case 4: setState(highCenter, lowCenter); break;
				case 5: setState(lowCenter, lowCenter); break;
				default: setState(lowCenter, lowCenter);
			}
			moveIt();

                        if(innerState == 3){
                                innerState = 0;
                        }else
                                innerState++;

		} else if(direction == CCLOCKWISE){
			switch(innerState){
				case 0: setState(highCenter, lowCenter); break;
				case 1: setState(lowLeft, lowRight); break;
				case 2: setState(lowCenter, highCenter); break;
				case 3: setState(lowRight, lowLeft); break;
				case 4: setState(highCenter, lowCenter); break;
				case 5: setState(lowCenter, lowCenter); break;
				default: setState(lowCenter, lowCenter);
			}
			moveIt();

                        if(innerState == 3){
                                innerState = 0;
                        }else
                                innerState++;

		} else
			state = IDLE;
	} else if(state == SEND_DATA){
		sendVerification();
	} else if(state == RESETTING){
		if(innerState == 0){
			setState(highCenter, 99);
			innerState++;
		} else if(innerState == 1){
			setState(lowCenter, 99);
			innerState++;
		} else if(innerState == 2){
			setState(99, highCenter);
			innerState++;
		} else if(innerState == 3){
			setState(lowCenter, lowCenter);
			innerState++;
		} else{
			state = IDLE;
		}
	}
}

void sendVerification(){
	state = SEND_VER;
	sendString("Q\r");
}

void stopMovement(){
	state = RESETTING;
	direction = RESETDIR;
	innerState = 0;
	setLowPos();
}

int getGaitStatus(){
    return state;
}

int getGaitDirection(){
    return direction;
}

void transmissionEndUART(){
    if(state < SEND_VER && state != IDLE) 
        nextStep();
    else if(state == SEND_VER)
        state = WAIT_VER;
    else
        state = IDLE;
}

void newDataUART(){
    updateGP2D12();
    if(state == WAIT_VER){
        char c = '+';
        while(dataAvailable())
                c= readUART();
        if(c == '.'){
            if( ((direction == FORWARD && getButton(1) && getGP2D12Distance() > 25.0) || (direction == CLOCKWISE && getButton(2)) || (direction == CCLOCKWISE && getButton(3)))){
                state = MOVING;
                nextStep();
            }else{
                if(innerState == 1 || innerState == 3){
                        state = IDLE;
                        resetPos();
                } else{
                        state = MOVING;
                        nextStep();
                }
            }

        } else
            sendVerification();
    }
}